/**
 *=============================================================================
 *  @copyright (C)      2025CCUT_1024lab_Damocles
 *
 *  @file    template.c
 *  @brief   封装模块代码规范
 *
 *  @note    使用方法
 *
 *  @attention 注意事项
 *-----------------------------------------------------------------------------
 *  @version    @author    @date    @note
 *  V1.0.0      Cherno    250808    done
 *
 *==============================================================================
 */

/*==============================Includes======================================*/
#include "crt_chassis.h"
/*==============================PrivateMacros=================================*/

/*==============================PrivateTypes==================================*/

/*==============================PrivateVariables==============================*/

/*==============================PrivateFunctionDeclarations===================*/

/*==============================FunctionPrototypes============================*/

/**
 * @brief : 描述该函数主要功能
 * @note : 注意事项
 *
 * @param : 参数名（与函数里的参数一致）  参数的描述
 * @return : 返回值的说明
 */

void Chassis::init(float _velocity_X_max, float _velocity_Y_max,
                   float _omega_max)
{
	velocity_X_max = _velocity_X_max;
	velocity_Y_max = _velocity_Y_max;
	omega_max = _omega_max;

	// 功率控制轮向电机PID
	power_limit_wheel.init(50.0f, 30.0f, 0.0f, 0.0f, wheel_max_output,
	                       wheel_max_output);
	power_limit_wheel.set_i_separate_threshold(20.0f);

	// 斜坡函数加减速速度X,Y,omega
	slope_velocity_X.init(0.05f, 0.1f);
	slope_velocity_Y.init(0.05f, 0.1f);
	slope_omega.init(0.05f, 0.1f);

	// 电机PID批量初始化
	for (uint8_t i = 0; i < 4; i++) {
		wheel_motor[i].pid_omega.init(200.0f, 20.0f, 0.0f, 0.0f,
		                              wheel_motor[i].get_out_max(),
		                              wheel_motor[i].get_out_max());
	}

	wheel_motor[0].init(&hcan1, DJI_ID_0x201);
	wheel_motor[1].init(&hcan1, DJI_ID_0x202);
	wheel_motor[2].init(&hcan1, DJI_ID_0x203);
	wheel_motor[3].init(&hcan1, DJI_ID_0x204);
}

void Chassis::power_calc_output()
{
	// 轮向电机PID计算
	power_limit_wheel.set_target(target_wheel_power);
	power_limit_wheel.set_ref(now_wheel_power);
	power_limit_wheel.calc();

	// 依靠PID输出限制电机输出最大值
	float out_max = power_limit_wheel.get_out();
	math_Limit(&out_max, 1.0f, wheel_max_output);
	for (int i = 0; i < 4; i++) {
		wheel_motor[i].pid_omega.set_out_max(out_max);
	}
}

void Chassis::power_limit()
{
// 根据当前功率限制调整PID
#if 1
	//    if(Referee->Get_Chassis_Power_Max() <= 60)
	// {
	// 	PID_Power_Limit_Wheel.Set_I_Separate_Threshold(20.0f);
	// }
	// else if (Referee->Get_Chassis_Power_Max() <= 90)
	// {
	// 	PID_Power_Limit_Wheel.Set_I_Separate_Threshold(30.0f);
	// }
	// else if (Referee->Get_Chassis_Power_Max() <= 120)
	// {
	// 	PID_Power_Limit_Wheel.Set_I_Separate_Threshold(45.0f);
	// }
	// else if (Referee->Get_Chassis_Power_Max() <= 150)
	// {
	// 	PID_Power_Limit_Wheel.Set_I_Separate_Threshold(65.0f);
	// }
	// // 要删除

	power_limit_wheel.set_i_separate_threshold(65.0f);
#endif
	// 功率计算
	// Now_Power = 0.3f * 20.0f / 16384 * Now_current * 60 / 2 * PI * Now_rpm /
	// 19;
	now_buf = 0.0f;
	// 单个轮向电机中间值获取
	for (uint8_t i = 0; i < 4; i++) {
		now_tump[i] =
		    math_Abs(wheel_motor[i].get_torque() * wheel_motor[i].get_omega());
		now_buf += now_tump[i];
	}
	now_power = POWER_CALC_CONSTANT * now_buf;

	// 当前轮向电机功率
	now_wheel_power = now_power;

	// 可使用的轮向电机功率
#if 1
	// Target_Wheel_Power =
	// Referee->Get_Chassis_Power_Max();
	// 要删除
	target_wheel_power = 150;
#endif

	power_calc_output();
}

void Chassis::kinematics_resolution() {}

void Chassis::output_to_dynamics()
{
	switch (type) {
		case (CHASSIS_DISABLE): {
			// 底盘失能
			for (uint8_t i = 0; i < 4; i++) {
				wheel_motor[i].set_control_method(OMEGA_CTRL);
				wheel_motor[i].pid_omega.set_integral_error(0.0f);
				wheel_motor[i].set_target_omega(0.0f);
			}
		} break;
		case (CHASSIS_OMNI): {
			// 全向轮模型
			for (uint8_t i = 0; i < 4; i++) {
				wheel_motor[i].set_control_method(OMEGA_CTRL);
			}

			// 全向轮模型限速
			if (velocity_X_max != 0) {
				math_Limit(&target_velocity_X, -velocity_X_max, velocity_X_max);
			}
			if (velocity_Y_max != 0) {
				math_Limit(&target_velocity_Y, -velocity_Y_max, velocity_Y_max);
			}
			if (omega_max != 0) {
				math_Limit(&target_omega, -omega_max, omega_max);
			}

			// 全向轮模型速度解算
			for (int i = 0; i < 4; i++) {
				// 计算每个轮子的速度分量
				// 公式: V_wheel = -Vx*sin(θ) + Vy*cos(θ) + ω*R
				// 其中θ是轮子安装角度，R是轮子到底盘中心的距离
				target_wheel_omega[i] =
				    (-arm_sin_f32(WHEEL_AZIMUTH[i]) *
				         slope_velocity_X.get_out() +
				     arm_cos_f32(WHEEL_AZIMUTH[i]) *
				         slope_velocity_Y.get_out() +
				     slope_omega.get_out() * WHEEL_TO_CORE_DISTANCE[i]) /
				    WHEEL_RADIUS;
			}
			for (int i = 0; i < 4; i++) {
				wheel_motor[i].set_target_omega(target_wheel_omega[i]);
			}
		} break;
		case (CHASSIS_MECANUM): {
			for (uint8_t i = 0; i < 4; i++) {
				wheel_motor[i].set_control_method(OMEGA_CTRL);
			}
		} break;
		case (CHASSIS_STEERING): {
			for (uint8_t i = 0; i < 4; i++) {
				wheel_motor[i].set_control_method(OMEGA_CTRL);
			}
		} break;
		default: {
		}
	}
}

/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/
